//---------------------------------------------------------------------------


#pragma hdrstop

#include "stdafx.h"
#include "Motor.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)

#define ADDINDEX_MRX(A, B) ((A + B) % MOTOR_RX_BUFF_SIZE)

MotorRXStruct MotorRX[BODYPART_NUM] = {0};
MotorDataStruct MotorData;
MotorWriteAddressStruct MotorAddress;
MotortTorqueStruct MotorTorque;
MotorWriteAddressStruct MotorTorqueAddress;
MotorScheduleStruct MotorSchedule;
HeadScheduleStruct HeadSchedule;

bool MotorWritten = false;
bool HeadWritten = false;
bool MotorWrittenFinish = false;

int TimerInterval_WriteMotor = TIMERINTERVAL_WRITEMOTOR;
int TimerInterval_ReadMotor = TIMERINTERVAL_READMOTOR;

//---------------------------------------------------------------------------
void Motor_Init(void)
{
	MotorRX_Init();
	MotorAddress_Init();
	MotorData_Init();
	MotorSchedule_Init();
	HeadSchedule_Init();
	MotorWritten = false;
	HeadWritten = false;
	MotorWrittenFinish = false;
}

void MotorRX_Init(void)
{
	int i, j;

	for(i=0; i < BODYPART_NUM; i++)
	{
		for(j=0; j < MOTOR_RX_BUFF_SIZE; j++)
		{
			MotorRX[i].Buff[j] = 0;
		}
		MotorRX[i].NowIndex = 0;
		MotorRX[i].EndIndex = 0;
	}
}

void MotorAddress_Init()
{
	MotorAddress.PacketHead[UPPER_BODY] = &(MotorData.PacketHead_UB[0]);
	MotorAddress.PacketHead[RIGHT_LEG] = &(MotorData.PacketHead_RL[0]);
	MotorAddress.PacketHead[LEFT_LEG] = &(MotorData.PacketHead_LL[0]);

	MotorAddress.Write[UPPER_BODY] = &(MotorData.Write_UB[0][0]);
	MotorAddress.Write[RIGHT_LEG] = &(MotorData.Write_RL[0][0]);
	MotorAddress.Write[LEFT_LEG] = &(MotorData.Write_LL[0][0]);

  MotorAddress.Checksum[UPPER_BODY] = &(MotorData.Checksum_UB);
	MotorAddress.Checksum[RIGHT_LEG] = &(MotorData.Checksum_RL);
	MotorAddress.Checksum[LEFT_LEG] = &(MotorData.Checksum_LL);

  MotorAddress.WtiteLength[UPPER_BODY] = &(MotorData.WtiteLength_UB);
	MotorAddress.WtiteLength[RIGHT_LEG] = &(MotorData.WtiteLength_RL);
	MotorAddress.WtiteLength[LEFT_LEG] = &(MotorData.WtiteLength_LL);
	////////////////////////////////////////////////////////////////////////////////
 	MotorTorqueAddress.PacketHead[UPPER_BODY] = &(MotorTorque.PacketHead_UB[0]);
	MotorTorqueAddress.PacketHead[RIGHT_LEG] = &(MotorTorque.PacketHead_RL[0]);
	MotorTorqueAddress.PacketHead[LEFT_LEG] = &(MotorTorque.PacketHead_LL[0]);

	MotorTorqueAddress.Write[UPPER_BODY] = &(MotorTorque.Write_UB[0][0]);
	MotorTorqueAddress.Write[RIGHT_LEG] = &(MotorTorque.Write_RL[0][0]);
	MotorTorqueAddress.Write[LEFT_LEG] = &(MotorTorque.Write_LL[0][0]);

  MotorTorqueAddress.Checksum[UPPER_BODY] = &(MotorTorque.Checksum_UB);
	MotorTorqueAddress.Checksum[RIGHT_LEG] = &(MotorTorque.Checksum_RL);
	MotorTorqueAddress.Checksum[LEFT_LEG] = &(MotorTorque.Checksum_LL);

  MotorTorqueAddress.WtiteLength[UPPER_BODY] = &(MotorTorque.WtiteLength_UB);
	MotorTorqueAddress.WtiteLength[RIGHT_LEG] = &(MotorTorque.WtiteLength_RL);
	MotorTorqueAddress.WtiteLength[LEFT_LEG] = &(MotorTorque.WtiteLength_LL);
}

void MotorData_Init(void)
{
	int i, j;
	unsigned char id = 0;

	MotorAddress_Init();
	// Unchangeable	when running process
	for(i=0; i < BODYPART_NUM; i++)
	{
		*(MotorAddress.PacketHead[i] + HEAD1) = 0xFF;
		*(MotorAddress.PacketHead[i] + HEAD2) = 0xFF;
		*(MotorAddress.PacketHead[i] + ID_SYNCWRITE) = 0xFE; // 0xFE, ID for SYNC WRITE
		*(MotorAddress.PacketHead[i] + TOTAL_LENGTH) = (1 + (MOTOR_WRITE_LENGTH-1)) * MotorNum[i] + 4;
		*(MotorAddress.PacketHead[i] + INSTRUCTION_SYNCWRITE) = 0x83; // 0x83, Instruction for SYNC WRITE
		*(MotorAddress.PacketHead[i] + FIRST_ADDRESS) = 0x1E; // 0x1E, First address to write (low byte of goal position)
		*(MotorAddress.PacketHead[i] + SINGLE_DATA_LENGTH) = (MOTOR_WRITE_LENGTH-1);  // 0x04, length of data to write
		//////////////////////////////////////////////////////////////////////////////////
		*(MotorTorqueAddress.PacketHead[i] + HEAD1) = 0xFF;
		*(MotorTorqueAddress.PacketHead[i] + HEAD2) = 0xFF;
		*(MotorTorqueAddress.PacketHead[i] + ID_SYNCWRITE) = 0xFE; // 0xFE, ID for SYNC WRITE
		*(MotorTorqueAddress.PacketHead[i] + TOTAL_LENGTH) = (1 + (MOTOR_TORQUE_LENGTH-1)) * MotorNum[i] + 4;
		*(MotorTorqueAddress.PacketHead[i] + INSTRUCTION_SYNCWRITE) = 0x83; // 0x83, Instruction for SYNC WRITE
		*(MotorTorqueAddress.PacketHead[i] + FIRST_ADDRESS) = 0x18; // 0x18, First address to write (torque)
		*(MotorTorqueAddress.PacketHead[i] + SINGLE_DATA_LENGTH) = (MOTOR_TORQUE_LENGTH-1);  // 0x01, length of data to write

		for(j=ID_SYNCWRITE; j < PACKET_HEAD_LENGTH; j++)
		{
			MotorData.Checksum_PacketHead[i] += *(MotorAddress.PacketHead[i] + j);
			MotorTorque.Checksum_PacketHead[i] += *(MotorTorqueAddress.PacketHead[i] + j);
		}

		for(j=0; j < MotorNum[i]; j++)
		{
			if(i == UPPER_BODY)
			{
				id = MotorID.InUB[j];
			}
			else if(i == RIGHT_LEG)
			{
				id = MotorID.InRL[j];
			}
			else
			{
				id = MotorID.InLL[j];
      }

			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + ID) = id;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + LOW_POSITION) = 0;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + HIGH_POSITION) = 0x20;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + LOW_SPEED) = 100;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + HIGH_SPEED) = 0x00;
			//////////////////////////////////////////////////////////////////////////////////
			*(MotorTorqueAddress.Write[i] + (j*MOTOR_TORQUE_LENGTH) + TORQUE_ID) = id;
			*(MotorTorqueAddress.Write[i] + (j*MOTOR_TORQUE_LENGTH) + TORQUE_ENABLED) = ON; // Torque: ON
		}

		*(MotorAddress.Checksum[i]) = 0;
		//////////////////////////////////////////////////////////////////////////////////
		*(MotorTorqueAddress.Checksum[i]) = 0;

		// Length = #PacketHead + #Write + Checksum
		*(MotorAddress.WtiteLength[i]) = PACKET_HEAD_LENGTH + (MotorNum[i] * MOTOR_WRITE_LENGTH) + 1;
		*(MotorTorqueAddress.WtiteLength[i]) = PACKET_HEAD_LENGTH + (MotorNum[i] * MOTOR_TORQUE_LENGTH) + 1;
	}

	// Changeable	when running process
	for(i=0; i < MOTOR_NUM; i++)
	{
		MotorData.Read[i][READ_ID] = MotorID.InUB[i];

		for(j=(READ_ID+1); j < MOTOR_READ_LENGTH; j++)
		{
			MotorData.Read[i][j] = 0;
		}
	}

	for(i=0; i < BODYPART_NUM; i++)
	{
		MotorData.Battery[i] = 0;
		MotorData.BatteryCount[i] = 0;
	}
}

void MotorSchedule_Init(void)
{
	int i,j;
	MotorSchedule.Enabled = false;
	MotorSchedule.NowIndex = 0;
 	MotorSchedule.EndIndex = 0;

	for(i=0; i < MOTOR_SCHEDULE_SIZE; i++)
	{
		for(j=0; j < MOTOR_SCHEDULE_LIST_LENGTH; j++)
		{
			MotorSchedule.List[i][j] = 0;
		}
	}

	MotorSchedule.Delay_ExecutedCount = 0;
	MotorSchedule.TorqueChanged = OFF;
	MotorScheduleSwitch(MotorSchedule.Enabled);
}

void HeadSchedule_Init(void)
{
	int i, j;
	HeadSchedule.NowIndex = 0;
 	HeadSchedule.EndIndex = 0;
	HeadSchedule.Delay_ExecutedCount = 0;

	for(i=0; i < HEAD_SCHEDULE_SIZE; i++)
	{
		for(j=0; j < HEAD_SCHEDULE_LIST_LENGTH; j++)
		{
			HeadSchedule.List[i][j] = 0;
		}
	}
}

void CalculateChecksum(void)
{
	unsigned char i, j, k;

	for(i=0; i < BODYPART_NUM; i++)
	{
		*(MotorAddress.Checksum[i]) = MotorData.Checksum_PacketHead[i];
		*(MotorTorqueAddress.Checksum[i]) = MotorTorque.Checksum_PacketHead[i];

		for(j=0; j < MotorNum[i]; j++)
		{
			for(k=0; k < MOTOR_WRITE_LENGTH; k++)
			{
				*(MotorAddress.Checksum[i]) = *(MotorAddress.Checksum[i]) + *(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + k);
			}
			for(k=0; k < MOTOR_TORQUE_LENGTH; k++)
			{
				*(MotorTorqueAddress.Checksum[i]) = *(MotorTorqueAddress.Checksum[i]) + *(MotorTorqueAddress.Write[i] + (j*MOTOR_TORQUE_LENGTH) + k);
			}
		}

		*(MotorAddress.Checksum[i]) = ~(*(MotorAddress.Checksum[i]));
		*(MotorTorqueAddress.Checksum[i]) = ~(*(MotorTorqueAddress.Checksum[i]));
	}
}

void CalculateReadChecksum(void)
{
	for(int i=0; i < MOTOR_READ_TOTAL_NUM; i++)
	{
		MotorData.Read[i][READ_CHECKSUM] = 0;

		for(int j=0; j < READ_CHECKSUM; j++)
		{
			MotorData.Read[i][READ_CHECKSUM] += MotorData.Read[i][j];
		}

		MotorData.Read[i][READ_CHECKSUM] = ~MotorData.Read[i][READ_CHECKSUM];
	}
}

void WriteMotor(BodyPartStatus part)
{
	CalculateChecksum();
	if((part == UPPER_BODY) || (part == BODYPART_NUM))
	{
		Comm_Write(MOTOR_UB_PORT, MotorAddress.PacketHead[UPPER_BODY], *(MotorAddress.WtiteLength[UPPER_BODY]));
		#ifdef bShow_MotorWrite
			if(bShow_MotorWrite)
			{
				AnsiString str_show = "Write UB: " + IntToStr(*(MotorAddress.PacketHead[UPPER_BODY]));
				for(int i=1; i < *(MotorAddress.WtiteLength[UPPER_BODY]); i++)
				{
					str_show = str_show + ", " + IntToStr(*(MotorAddress.PacketHead[UPPER_BODY]+i));
				}
				Memo_ShowInfo->Lines->Add(str_show);
			}
		#endif
	}

	if((part == RIGHT_LEG) || (part == BODYPART_NUM))
	{
		Comm_Write(MOTOR_RL_PORT, MotorAddress.PacketHead[RIGHT_LEG], *(MotorAddress.WtiteLength[RIGHT_LEG]));
		#ifdef bShow_MotorWrite
			if(bShow_MotorWrite)
			{
				AnsiString str_show = "Write RL: " + IntToStr(*(MotorAddress.PacketHead[RIGHT_LEG]));
				for(int i=1; i < *(MotorAddress.WtiteLength[RIGHT_LEG]); i++)
				{
					str_show = str_show + ", " +IntToStr(*(MotorAddress.PacketHead[RIGHT_LEG]+i));
				}
				Memo_ShowInfo->Lines->Add(str_show);
			}
		#endif
	}

	if((part == LEFT_LEG) || (part == BODYPART_NUM))
	{
		Comm_Write(MOTOR_LL_PORT, MotorAddress.PacketHead[LEFT_LEG], *(MotorAddress.WtiteLength[LEFT_LEG]));
		#ifdef bShow_MotorWrite
			if(bShow_MotorWrite)
			{
				AnsiString str_show = "Write LL: " + IntToStr(*(MotorAddress.PacketHead[LEFT_LEG]));
				for(int i=1; i < *(MotorAddress.WtiteLength[LEFT_LEG]); i++)
				{
					str_show = str_show + ", " +IntToStr(*(MotorAddress.PacketHead[LEFT_LEG]+i));
				}
				Memo_ShowInfo->Lines->Add(str_show);
			}
		#endif
	}
}

void WriteTorque(BodyPartStatus part)
{
	CalculateChecksum();
	if((part == UPPER_BODY) || (part == BODYPART_NUM))
	{
		Comm_Write(MOTOR_UB_PORT, MotorTorqueAddress.PacketHead[UPPER_BODY], *(MotorTorqueAddress.WtiteLength[UPPER_BODY]));
	}

	if((part == RIGHT_LEG) || (part == BODYPART_NUM))
	{
		Comm_Write(MOTOR_RL_PORT, MotorTorqueAddress.PacketHead[RIGHT_LEG], *(MotorTorqueAddress.WtiteLength[RIGHT_LEG]));
	}

	if((part == LEFT_LEG) || (part == BODYPART_NUM))
	{
		Comm_Write(MOTOR_LL_PORT, MotorTorqueAddress.PacketHead[LEFT_LEG], *(MotorTorqueAddress.WtiteLength[LEFT_LEG]));
	}
}

bool ReadMotor(unsigned char id_index)
{
	if(id_index >= 0 && id_index < MOTOR_NUM)
	{
		unsigned char id = MotorData.Read[id_index][READ_ID];
		int i, temp_part, temp_no;
//		BodyPartStatus bodypart;
		TYbCommDevice *commport = NULL;
		unsigned char checksum = 0;
		unsigned char read_packet[8] = {0xFF, 0xFF, 0x00, 0x04, 0x02, 0x24, 0x07, 0x00};

		MotorData.Read[id_index][READ_VOLTAGE] = 0;

		read_packet[2] = id;
		for(i=2; i < 7; i++)
		{
			checksum += read_packet[i];
		}
		read_packet[7] = ~checksum;

		if(IDtoPartNo(id, &temp_part, &temp_no) == false)
		{
			return false;
		}

		if(temp_part == UPPER_BODY)
		{
//			bodypart = UPPER_BODY;
			commport = MOTOR_UB_PORT;
		}
		else if(temp_part == RIGHT_LEG)
		{
//			bodypart = RIGHT_LEG;
			commport = MOTOR_RL_PORT;
		}
		else if(temp_part == LEFT_LEG)
		{
//			bodypart = LEFT_LEG;
			commport = MOTOR_LL_PORT;
		}

		if(commport == NULL)
		{
			return false;
		}

		Comm_Write(commport, read_packet, 8);
		Delay_ms(2);
		CheckMotorRX(UPPER_BODY);
		CheckMotorRX(RIGHT_LEG);
		CheckMotorRX(LEFT_LEG);

		return true;
	}
	return false;
}

void CheckMotorRX(BodyPartStatus bodypart)
{
	TimeDifferenceStrcut T1 = {0,0,0};
	TYbCommDevice *commport = NULL;
	int num_nondealed_data = 0;
	int num_read_byte = 0;
	AnsiString str_show = "MotorRX ";

	if(bodypart == UPPER_BODY)
	{
		str_show += "UpperBody:";
		commport = MOTOR_UB_PORT;
	}
	else if(bodypart == RIGHT_LEG)
	{
		str_show += "RightLeg:";
		commport = MOTOR_RL_PORT;
	}
	else if(bodypart == LEFT_LEG)
	{
		str_show += "LeftLeg:";
		commport = MOTOR_LL_PORT;
	}

	T1.Past = GetCPUTime();
	num_read_byte = commport->Read(CommBuff, COMM_BUFF_SIZE);
//	if(num_read_byte > 0)
//	{
//		T1.Now = GetCPUTime();
//		CalculateTimeDifference_ms(&T1);
//		Memo_ShowInfo->Lines->Add(IntToStr(num_read_byte)+", t: "+FormatFloat("0.00", T1.dT));
//	}
	for(int i=0; i<num_read_byte; i++)
	{
		MotorRX[bodypart].Buff[MotorRX[bodypart].EndIndex] = CommBuff[i];
		MotorRX[bodypart].EndIndex = ADDINDEX_MRX(MotorRX[bodypart].EndIndex,1);
		#ifdef bShow_MotorRead
			if(bShow_MotorRead)
			{
				str_show = str_show + " " + IntToStr(CommBuff[i]);
			}
		#endif
	}
	#ifdef bShow_MotorRead
		if(bShow_MotorRead)
		{
			Memo_ShowInfo->Lines->Add(str_show);
		}
	#endif

	if(MotorRX[bodypart].NowIndex <= MotorRX[bodypart].EndIndex)
	{
		num_nondealed_data = MotorRX[bodypart].EndIndex - MotorRX[bodypart].NowIndex;
	}
	else
	{
		num_nondealed_data = MOTOR_RX_BUFF_SIZE - (MotorRX[bodypart].NowIndex - MotorRX[bodypart].EndIndex);
	}

	while(num_nondealed_data >= 13)
	{
		if((MotorRX[bodypart].Buff[MotorRX[bodypart].NowIndex] == 255) &&
			 (MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,1)] == 255) &&
//			 (MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,2)] == id) &&
			 (MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,3)] == 9))
		{
			unsigned char checksum = 0;
			for(int i=2; i<12; i++)
			{
				checksum += MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,i)];
			}
			checksum = ~checksum;
			if(checksum == MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,12)])
			{
				int id_index = ID2Index(MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,2)]);
				if(id_index >= 0 && id_index < MOTOR_NUM)
				{
				MotorData.Read[id_index][READ_LOW_POSITION] = MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,5)];
				MotorData.Read[id_index][READ_HIGH_POSITION] = MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,6)];
				MotorData.Read[id_index][READ_LOW_LOADING] = MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,9)];
				MotorData.Read[id_index][READ_HIGH_LOADING] = MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,10)];
				MotorData.Read[id_index][READ_VOLTAGE] = MotorRX[bodypart].Buff[ADDINDEX_MRX(MotorRX[bodypart].NowIndex,11)];
        }
				MotorRX[bodypart].NowIndex = MotorRX[bodypart].NowIndex + 13;
			}
			else
			{
				MotorRX[bodypart].NowIndex = ADDINDEX_MRX(MotorRX[bodypart].NowIndex,1);
			}
		}
		else
		{
			MotorRX[bodypart].NowIndex = ADDINDEX_MRX(MotorRX[bodypart].NowIndex,1);
		}

		if (MotorRX[bodypart].NowIndex <= MotorRX[bodypart].EndIndex)
		{
			num_nondealed_data = MotorRX[bodypart].EndIndex - MotorRX[bodypart].NowIndex;
		}
		else
		{
			num_nondealed_data = MOTOR_RX_BUFF_SIZE - (MotorRX[bodypart].NowIndex - MotorRX[bodypart].EndIndex);
		}
	}

//	T1.Now = GetCPUTime();
//	CalculateTimeDifference_us(&T1);
}

void MotorScheduleSwitch(bool enabled)
{
	if(enabled == true)
	{
		MotorSchedule.Enabled = true;
		//TimerStart(&TimerSub);
//		#ifdef SCHEDULE_UPDATE_TIMER
//			TIM_Cmd(SCHEDULE_UPDATE_TIMER, ENABLE);
//		#endif
	}
	else
	{
		MotorSchedule.Enabled = false;
		//TimerStop(&TimerSub);
//		#ifdef SCHEDULE_UPDATE_TIMER
//			TIM_Cmd(SCHEDULE_UPDATE_TIMER, DISABLE);
//		#endif
	}
}

void Update_MotorSchedule_to_MotorData(void)
{
	int i, index;
	unsigned char motor_ID;
	int part, no;

	MotorWritten = false;

	// Decide what to do
	if(MotorSchedule.NowIndex != MotorSchedule.EndIndex)
	{
		#ifdef bShow_ScheduleIndex
			if(bShow_ScheduleIndex)
			{
				Memo_ShowInfo->Lines->Add("NowIndex: " + IntToStr(MotorSchedule.NowIndex) +
																	", EndIndex: " + IntToStr(MotorSchedule.EndIndex));
			}
		#endif
    if(MotorSchedule.Delay_ExecutedCount > 0)
		{
			MotorSchedule.Delay_ExecutedCount--;
			if(MotorSchedule.Delay_ExecutedCount == 0)
			{
				MotorSchedule.NowIndex = (MotorSchedule.NowIndex + 1) % MOTOR_SCHEDULE_SIZE;
			}
		}
		else
		{
			// Update MotorData.Write, Torque or steptime, until INSTRUCTION_MODE is InsertDelayCount
			while((MotorSchedule.NowIndex != MotorSchedule.EndIndex) &&
						(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] != INSERT_DELAY_COUNT))
			{
        if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] <= MOTOR_MAX_ID)
				{
					// if INSTRUCTION_MODE < SetPosition, then INSTRUCTION_MODE = motor ID
					// Therefore, MotorSchedule.List[MotorShedule.NowIndex] = {ID, LowPosion, HighPosition, LowSpeed, HighSpeed}
					motor_ID = MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE];
					if(IDtoPartNo(motor_ID, &part, &no) == true)
					{
						if((*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + ID)) == motor_ID)
						{
							// Add correction value to motor data, if necessary
							index = ID2Index(motor_ID);
							if(index >= 0 && index < MOTOR_NUM)
							{
								if(MotorCorrection_Intrinsic.InUB[ID2Index(motor_ID)] == 0 &&
									 MotorCorrection_Feedback.InUB[ID2Index(motor_ID)] == 0)
								{
									for(i=LOW_POSITION; i < MOTOR_WRITE_LENGTH; i++)
									{
										*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + i) = MotorSchedule.List[MotorSchedule.NowIndex][i];
									}
								}
								else
								{
									unsigned short temp = MotorSchedule.List[MotorSchedule.NowIndex][LOW_POSITION] | (MotorSchedule.List[MotorSchedule.NowIndex][HIGH_POSITION] << 8);
									temp = temp + MotorCorrection_Intrinsic.InUB[ID2Index(motor_ID)] +
																MotorCorrection_Feedback.InUB[ID2Index(motor_ID)];
									temp = (temp <= MAX_MOTOR_POSITION) * temp +
												 (temp > MAX_MOTOR_POSITION) * MAX_MOTOR_POSITION;
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + LOW_POSITION) = temp & 0x00FF;
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + HIGH_POSITION) = (temp & 0xFF00) >> 8;
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + LOW_SPEED) = MotorSchedule.List[MotorSchedule.NowIndex][LOW_SPEED];
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + HIGH_SPEED) = MotorSchedule.List[MotorSchedule.NowIndex][HIGH_SPEED];
								}
								// Set torque
								*(MotorTorqueAddress.Write[part] + (no * MOTOR_TORQUE_LENGTH) + TORQUE_ENABLED) = ON;
								// Motor data are updated completely
								MotorWritten = true;
							}
							else
							{
								#ifdef bShow_ScheduleError
									if(bShow_ScheduleError)
										Memo_ShowInfo->Lines->Add("Error of Update_MotorSchedule: ID2Index < 0, ID is " + IntToStr(motor_ID));
								#endif
              }
						} // End if: motor_ID recheck
						else
						{
							#ifdef bShow_ScheduleError
								if(bShow_ScheduleError)
									Memo_ShowInfo->Lines->Add("Error of Update_MotorSchedule: ID check error, ID is " + IntToStr(motor_ID));
							#endif
            }
					} // End if: motor_ID check
					else
					{
						#ifdef bShow_ScheduleError
							if(bShow_ScheduleError)
								Memo_ShowInfo->Lines->Add("Error of Update_MotorSchedule: IDtoPartNo return false, ID is " + IntToStr(motor_ID));
						#endif
					}
				} // End if: SetPosition
				else if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] < SET_TORQUE)
				{
          // if INSTRUCTION_MODE < SetTorque, then INSTRUCTION_MODE-TORQUE_BASE = motor ID
					motor_ID = MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] - TORQUE_BASE;
					if(IDtoPartNo(motor_ID, &part, &no))
					{
						if((*(MotorTorqueAddress.Write[part] + (no * MOTOR_TORQUE_LENGTH) + TORQUE_ID)) == motor_ID)
						{
							*(MotorTorqueAddress.Write[part] + (no * MOTOR_TORQUE_LENGTH) + TORQUE_ENABLED) = MotorSchedule.List[MotorSchedule.NowIndex][1];
							MotorSchedule.TorqueChanged = ON;
						}
						else
						{
							#ifdef bShow_ScheduleError
								if(bShow_ScheduleError)
									Memo_ShowInfo->Lines->Add("Error of Update_MotorSchedule(Torque): ID check error, ID is " + IntToStr(motor_ID));
							#endif
            }
					}
					else
					{
						#ifdef bShow_ScheduleError
							if(bShow_ScheduleError)
								Memo_ShowInfo->Lines->Add("Error of Update_MotorSchedule(Torque): IDtoPartNo return false, ID is " + IntToStr(motor_ID));
						#endif
          }
				} // End if: SetTorque
				else if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] == SET_STEP_TIME)
				{
					TimerInterval_WriteMotor = MotorSchedule.List[MotorSchedule.NowIndex][1] * 10;
					if(TimerInterval_WriteMotor > 2000)
					{
						TimerInterval_WriteMotor = 2000;
					}
					else if(TimerInterval_WriteMotor < 30)
					{
						TimerInterval_WriteMotor = 30;
          }
					TimerStop(&TimerMain);
					TimerMain.Interval = TimerInterval_WriteMotor;
					TimerStart(&TimerMain);
				}
				MotorSchedule.NowIndex = (MotorSchedule.NowIndex + 1) % MOTOR_SCHEDULE_SIZE;
			} // End while

			if(MotorSchedule.NowIndex != MotorSchedule.EndIndex)
			{	// This if-statement is replenished on 2012/07/30 to prevent out-of-order index.
				if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] == INSERT_DELAY_COUNT)
				{
					MotorSchedule.Delay_ExecutedCount = MotorSchedule.List[MotorSchedule.NowIndex][1] - 1;
					if(MotorSchedule.Delay_ExecutedCount <= 0)
					{
						 MotorSchedule.NowIndex = (MotorSchedule.NowIndex + 1) % MOTOR_SCHEDULE_SIZE;
					}
				}
			}	// End if: NowIndex != EndIndex
		} // End if: Delay = 0
	} // End if: Updata schedule (Decide what to do)

	if((MotorSchedule.Delay_ExecutedCount == 0) && (MotorSchedule.NowIndex == MotorSchedule.EndIndex))
	{
		MotorSchedule.NowIndex = 0;
		MotorSchedule.EndIndex = 0;
	}

	if(MotorSchedule.NowIndex == MotorSchedule.EndIndex)
	{
		if(Motion.Continue_Eanble == true || Motion.Mode > 100)
		{
			Motion.Doing = Motion.Continue_Set;
    }
		else if(MotorSchedule.Delay_ExecutedCount == 0)
		{
      Motion.Doing = NO_MOTION;
		}
		#ifdef bShow_MotionDoing
			if(bShow_MotionDoing)
			{
        Memo_ShowInfo->Lines->Add("Motion Doing: " + MotionMode2MotionName(Motion.Doing) + ", " + IntToStr(Motion.Doing));
			}
		#endif
	}

	// Change the torque
	if(MotorSchedule.TorqueChanged == ON)
	{
    WriteTorque(BODYPART_NUM);
    MotorSchedule.TorqueChanged = OFF;
  }

	if(MotorWritten == true)
	{
		WriteMotor(BODYPART_NUM);
	}
}

void Update_HeadSchedule_to_MotorData(void)
{
	int i;
	unsigned char motor_ID;
	int part, no;
	HeadWritten = false;


	// Decide what to do
	if(HeadSchedule.NowIndex != HeadSchedule.EndIndex)
	{
		if(HeadSchedule.Delay_ExecutedCount > 0)
		{
			HeadSchedule.Delay_ExecutedCount--;
			if(HeadSchedule.Delay_ExecutedCount == 0)
			{
				HeadSchedule.NowIndex = (HeadSchedule.NowIndex + 1) % HEAD_SCHEDULE_SIZE;
			}
		}
		else
		{
			// Update MotorData.Write, Torque or steptime, until INSTRUCTION_MODE is InsertDelayCount
			while((HeadSchedule.NowIndex != HeadSchedule.EndIndex) &&
						(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] != INSERT_DELAY_COUNT))
			{
				if(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] < MOTOR_MAX_ID)
				{
					// if INSTRUCTION_MODE < SetPosition, then INSTRUCTION_MODE = motor ID
					// Therefore, HeadSchedule.List[HeadShedule.NowIndex] = {ID, LowPosion, HighPosition, LowSpeed, HighSpeed}
					motor_ID = HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE];
					if(IDtoPartNo(motor_ID, &part, &no))
					{
						if((*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + ID)) == motor_ID)
						{
							// Add correction value to motor data, if necessary
							int index = ID2Index(motor_ID);
							if(index >= 0)
							{
								if(MotorCorrection_Intrinsic.InUB[index] == 0)
								{
									for(i=LOW_POSITION; i < MOTOR_WRITE_LENGTH; i++)
									{
										*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + i) = HeadSchedule.List[HeadSchedule.NowIndex][i];
									}
								}
								else
								{
									int temp = HeadSchedule.List[HeadSchedule.NowIndex][LOW_POSITION] | (HeadSchedule.List[HeadSchedule.NowIndex][HIGH_POSITION] << 8);
									temp = temp + MotorCorrection_Intrinsic.InUB[index];
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + LOW_POSITION) = temp & 0x00FF;
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + HIGH_POSITION) = (temp & 0xFF00) >> 8;
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + LOW_SPEED) = HeadSchedule.List[HeadSchedule.NowIndex][LOW_SPEED];
									*(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + HIGH_SPEED) = HeadSchedule.List[HeadSchedule.NowIndex][HIGH_SPEED];
								}
								// Head data are updated completely
								HeadWritten = true;
							}
							else
							{
								#ifdef bShow_ScheduleError
									if(bShow_ScheduleError)
										Memo_ShowInfo->Lines->Add("Error of Update_HeadSchedule: ID2Index < 0, ID is " + IntToStr(motor_ID));
								#endif
							}
						} // End if: motor_ID recheck
						else
						{
							#ifdef bShow_ScheduleError
								if(bShow_ScheduleError)
									Memo_ShowInfo->Lines->Add("Error of Update_HeadSchedule: ID check error, ID is " + IntToStr(motor_ID));
							#endif
						}
					}
					else
					{
						#ifdef bShow_ScheduleError
							if(bShow_ScheduleError)
								Memo_ShowInfo->Lines->Add("Error of Update_HeadSchedule: IDtoPartNo return false, ID is " + IntToStr(motor_ID));
						#endif
          } // End else: motor_ID recheck
				}	// End if: SetPosition
//				else if(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] == SET_STEP_TIME)
//				{
//					TimerInterval_WriteHead =  HeadSchedule.List[HeadSchedule.NowIndex][1] * 10;
//				}

				HeadSchedule.NowIndex = (HeadSchedule.NowIndex + 1) % HEAD_SCHEDULE_SIZE;
			} // End while

			if(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] == INSERT_DELAY_COUNT)
			{
				HeadSchedule.Delay_ExecutedCount = HeadSchedule.List[HeadSchedule.NowIndex][1] - 1;
				if(HeadSchedule.Delay_ExecutedCount <= 0)
				{
					 HeadSchedule.NowIndex = (HeadSchedule.NowIndex + 1) % HEAD_SCHEDULE_SIZE;
				}
			}
		} // End if: Delay = 0
	}	// End if: Updata schedule (Decide what to do)


	if((HeadSchedule.Delay_ExecutedCount == 0) && (HeadSchedule.NowIndex == HeadSchedule.EndIndex))
	{
		HeadSchedule.NowIndex = 0;
		HeadSchedule.EndIndex = 0;
	}
}


/*******************************************************************************
* Function Name  : Insert_MotorSchedule
* Description    : None
* Input          : data0 ~ data4 (usually: ID, LP, HP, LS, HS)
* Output         : None
* Return         : Successed / Fail
* Attention		 	 : None
*******************************************************************************/
void Add_motor_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4)
{
	MotorSchedule.List[MotorSchedule.EndIndex][0] = data0;
	MotorSchedule.List[MotorSchedule.EndIndex][1] = data1;
	MotorSchedule.List[MotorSchedule.EndIndex][2] = data2;
	MotorSchedule.List[MotorSchedule.EndIndex][3] = data3;
	MotorSchedule.List[MotorSchedule.EndIndex][4] = data4;
	MotorSchedule.EndIndex = (MotorSchedule.EndIndex + 1) % MOTOR_SCHEDULE_SIZE;
}

/*******************************************************************************
* Function Name  : Insert_HeadSchedule
* Description    : None
* Input          : data0 ~ data4 (usually: ID, LP, HP, LS, HS)
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void Add_head_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4)
{
 	HeadSchedule.List[HeadSchedule.EndIndex][0] = data0;
  HeadSchedule.List[HeadSchedule.EndIndex][1] = data1;
  HeadSchedule.List[HeadSchedule.EndIndex][2] = data2;
  HeadSchedule.List[HeadSchedule.EndIndex][3] = data3;
  HeadSchedule.List[HeadSchedule.EndIndex][4] = data4;

  HeadSchedule.EndIndex = (HeadSchedule.EndIndex + 1) % HEAD_SCHEDULE_SIZE;
}

/*******************************************************************************
* Function Name  : IDtoPartNo
* Description    : None
* Input          : id
* Output         : part & no
* Return         : None
* Attention		 	 : For MotorData.Write
*******************************************************************************/
bool IDtoPartNo(unsigned char id, int *part, int *no)
{
	bool is_motor_id = false;
	unsigned char i = 0;

	for(i=0; i < MOTOR_NUM; i++)
	{
		if(id == MotorID.InUB[i])
		{
			is_motor_id = true;
      break;
    }
	}

	if(is_motor_id)
	{
		if(i < MOTOR_NUM_UB)
		{
			*part = UPPER_BODY;
			*no = i;
			return true;
		}
		else if(i < MOTOR_NUM_UB + MOTOR_NUM_RL)
		{
			*part = RIGHT_LEG;
			*no = i - MOTOR_NUM_UB;
			return true;
		}
		else if(i < MOTOR_NUM)
		{
			*part = LEFT_LEG;
			*no = i - MOTOR_NUM_UB - MOTOR_NUM_RL;
			return true;
		}
	}

	*part = 0;
	*no = 0;
	return false;
}

/*******************************************************************************
* Function Name  : ID2CorrectionIndex
* Description    : None
* Input          : id
* Output         : correction motor index
* Return         : None
* Attention		 	 : For MotorData.Write
*******************************************************************************/
int ID2Index(unsigned char id)
{
	int i=0;
	for(i=0; i < MOTOR_NUM; i++)
	{
		if(MotorID.InUB[i] == id)
		{
			return i;
		}
  }
	return -1;
}

void ShowMotorSchedule(TMemo *memo_show)
{
	AnsiString str_show = "";
	memo_show->Lines->Add("MotorSchedule NowIndex: " + IntToStr(MotorSchedule.NowIndex) +
												", EndIndex: " + IntToStr(MotorSchedule.EndIndex));
	for(int i = MotorSchedule.NowIndex; i < MotorSchedule.EndIndex; i++)
//	for(int i = 0; i < MOTOR_SCHEDULE_SIZE; i++)
	{
		str_show = "Index " + IntToStr(i) + ":";
		for(int j=0; j < MOTOR_SCHEDULE_LIST_LENGTH; j++)
		{
      str_show += " " + IntToStr(MotorSchedule.List[i][j]);
		}
		memo_show->Lines->Add(str_show);
	}
  memo_show->Lines->Add("");
}

void ShowHeadSchedule(TMemo *memo_show)
{
	AnsiString str_show = "";
	memo_show->Lines->Add("HeadSchedule NowIndex: " + IntToStr(HeadSchedule.NowIndex) +
												", EndIndex: " + IntToStr(HeadSchedule.EndIndex));
	for(int i = HeadSchedule.NowIndex; i < HeadSchedule.EndIndex; i++)
//	for(int i = 0; i < HEAD_SCHEDULE_SIZE; i++)
	{
		str_show = "Index " + IntToStr(i) + ":";
		for(int j=0; j < HEAD_SCHEDULE_LIST_LENGTH; j++)
		{
			str_show += " " + IntToStr(HeadSchedule.List[i][j]);
		}
		memo_show->Lines->Add(str_show);
	}
  memo_show->Lines->Add("");
}

void ShowMotorWriteData(TMemo *memo_show)
{
	AnsiString str_show = "";
	AnsiString str_part = "";
	for(int i=0; i < BODYPART_NUM; i++)
	{
		if(i == UPPER_BODY)
		{
			str_part = "UpperBody";
		}
		else if(i == RIGHT_LEG)
		{
			str_part = "RightLeg";
    }
		else if(i == LEFT_LEG)
		{
			str_part = "LeftLeg";
		}
		else
		{
			str_part = "Bodypart Undefined";
		}

		str_show = str_part + " PacketHead:\r\n  ";
		for(int j=0; j < PACKET_HEAD_LENGTH; j++)
		{
			str_show += " " + IntToStr(*(MotorAddress.PacketHead[i] + j));
		}
		memo_show->Lines->Add(str_show);

		for(int j=0; j < MotorNum[i]; j++)
		{
			str_show = "ID: " + IntToStr(*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + ID));
			str_show += ", Pos: " + IntToStr((*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + LOW_POSITION)) + ((*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + HIGH_POSITION)) << 8));
			str_show += ", Speed: " + IntToStr((*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + LOW_SPEED)) + ((*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + HIGH_SPEED)) << 8));
			memo_show->Lines->Add(str_show);
		}
		memo_show->Lines->Add("CheckSum: " + IntToStr(*(MotorAddress.Checksum[i]))+ "\r\n");
	}
}

void ShowMotorReadData(TMemo *memo_show)
{
	AnsiString str_show = "";
	for(int i=0; i < MOTOR_NUM; i++)
	{
		str_show = "Read ID: " + IntToStr(MotorData.Read[i][READ_ID]);
		str_show += ", Pos: " + IntToStr(MotorData.Read[i][READ_LOW_POSITION] + (MotorData.Read[i][READ_HIGH_POSITION] << 8));
		str_show += ", Loading: " + IntToStr(MotorData.Read[i][READ_LOW_LOADING] + (MotorData.Read[i][READ_HIGH_LOADING] << 8));
		str_show += ", Voltage: " + IntToStr(MotorData.Read[i][READ_VOLTAGE]);
		memo_show->Lines->Add(str_show);
	}
}

void ShowMotorRX(TMemo *memo_show)
{
	AnsiString str_show = "";
	AnsiString str_part = "";

	for(int i=0; i < BODYPART_NUM; i++)
	{
    str_show = "";
		if(i == UPPER_BODY)
		{
			str_part = "UpperBody";
		}
		else if(i == RIGHT_LEG)
		{
			str_part = "RightLeg";
    }
		else if(i == LEFT_LEG)
		{
			str_part = "LeftLeg";
		}
		memo_show->Lines->Add("MotorRX " + str_part + ":");

		str_show = IntToStr(0) + ": ";
		for(int j=0; j < MOTOR_RX_BUFF_SIZE; j++)
		{
			if(j != 0 && j % 10 == 0)
			{
				memo_show->Lines->Add(str_show);
				str_show = IntToStr(j) + ": ";
			}
			str_show += IntToStr(MotorRX[i].Buff[j]) + " ";
		}
		memo_show->Lines->Add(str_show + "\r\n");
  }
}
